###### Client in Machine order to UI Operator to control KT-Robot (JSON) 
###### Author: ZHENG
###### Date: 2023-11-27
###### START

from PyQt5 import QtCore, QtWidgets
from PyQt5.QtWidgets import QApplication, QSlider, QVBoxLayout, QStackedWidget, QWidget, QComboBox, QPushButton, QListView
from PyQt5.QtCore import Qt, pyqtSlot, pyqtSignal 
from Ui_main import Ui_MainWindow
from PyQt5.QtGui import QIcon, QTextCursor
import socket
import json, os, re, socket, sys, inspect
import struct
from struct import pack, unpack
from serial import Serial
import math, time,json, logging

FolderPath = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
robot_logger=logging.getLogger("Controler-Driver")
robot_logger.setLevel(logging.DEBUG)

class MainWindow_main_zubo(QtWidgets.QMainWindow, Ui_MainWindow):

    def __init__(self):
        super(MainWindow_main_zubo, self).__init__()
        self.setupUi(self)
        self.initUI()
        self.initSingnal()
        self._combox()
        self.robot_type = 1
        self.global_state = [1,1,1]
        self.IO = [0,1,1,1,1,1,1,1]
        self.default_speed = 1000
        self.angles_6 = 0
        
    def initUI(self):        
        self.label_12.setText("通讯:(TCP)")
        self.label_10.setText("Server_IP")
        self.label_11.setText("Server_Port")
        self.btnConnect.setText("未连接")
        self.label_13.setText("Robot状态：")
        self.label_14.setText("运动控制：")        
        self.btn_Jog.setText("轴角坐标")
        self.btn_Coordinate.setText("世界坐标")
        self.label_17.setText("状态监控：")
        self.label_15.setText("API函数:")
        self.label_2.setText("速度")
        self.labelspeed_world.setText("速度")      
        self.btnEmergence.setText("急停")
        self.btnHome.setText("机械复位")
        self.btnZero.setText("条件复位")
        self.btnStop.setText("停止")
        self.btnDO0.setText("DO0")
        self.btnDO1.setText("DO1")
        self.btnStatus.setText("状态获取")
        self.labelIO.setText("IO:")
        self.labelStatus.setText("状态")
        self.labelPose.setText("位姿")

        self.btn_pyExecute.setText("执行")
        self.btn_pyStop.setText("停止")
        self.btn_sender.setText("发送指令")
        self.btn_pyClear.setText("清屏")

        self.label_3.setText("轴J1 :(度)")
        self.label_4.setText("轴J2 : (度)")
        self.label_5.setText("轴J3 : (度)")
        self.label_6.setText("轴J4 : (度)")
        self.label_7.setText("轴J5 : (度)")
        self.label_8.setText("轴J6 : (度)")
        self.label_9.setText("轴J7 : (度)")

        self.label_x.setText("坐标 X : ()")
        self.label_y.setText("坐标 Y : ()")
        self.label_z.setText("坐标 Z : ()")
        self.label_a.setText("坐标 RX : ()")
        self.label_b.setText("坐标 RY : ()")
        self.label_c.setText("坐标 RZ : ()")

        self.lineEdit_1.setText("0")
        self.lineEdit_2.setText("0")
        self.lineEdit_3.setText("0")
        self.lineEdit_4.setText("0")
        self.lineEdit_5.setText("0")
        self.lineEdit_6.setText("0")
        self.lineEdit_7.setText("0")

        self.horizontalSlider_1.setRange(-100, 160)
        self.horizontalSlider_2.setRange(-40, 70)
        self.horizontalSlider_3.setRange(-170, 60)
        self.horizontalSlider_4.setRange(-350, 350)
        self.horizontalSlider_5.setRange(-205, 36)
        self.horizontalSlider_6.setRange(-360, 360)
        self.horizontalSlider_7.setRange(0,10)

        self.horizontalScrollBar.setRange(0, 500)


        #### StackedWeight
        # self.stackedWidget = QStackedWidget()
        # self.page = QWidget()
        # self.page_2 = QWidget()
        # self.stackedWidget.addWidget(self.page)    
        # self.stackedWidget.addWidget(self.page_2)       

    def initSingnal(self):
        self.horizontalSlider_1.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_1))
        self.horizontalSlider_2.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_2))
        self.horizontalSlider_3.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_3))
        self.horizontalSlider_4.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_4))
        self.horizontalSlider_5.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_5))
        self.horizontalSlider_6.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_6))
        self.horizontalSlider_7.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_7))
        self.horizontalScrollBar.valueChanged.connect(lambda: self.onSliderReleased(self.label_speed))

        self.horizontalSlider_1world.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_1world))
        self.horizontalSlider_2world.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_2world))
        self.horizontalSlider_3world.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_3world))
        self.horizontalSlider_4world.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_4world))
        self.horizontalSlider_5world.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_5world))
        self.horizontalSlider_6world.sliderReleased.connect(lambda: self.onSliderReleased(self.lineEdit_6world))
        self.horizontalScrollBar_2.valueChanged.connect(lambda: self.onSliderReleased(self.label_speed_world))       

        self.btnStatus.clicked.connect(self.onBtnStatus)
        self.btnConnect.clicked.connect(self.onStateChanged)

        self.btn_Jog.clicked.connect(lambda: self.stackedWidget.setCurrentIndex(0))  
        self.btn_Coordinate.clicked.connect(lambda: self.stackedWidget.setCurrentIndex(1))

        self.btn_pyExecute.clicked.connect(self.execute_code)
        self.btn_pyStop.clicked.connect(self.stop_code)
        self.btn_sender.clicked.connect(self.onButtonClick)
        self.btn_pyClear.clicked.connect(self.clear_code)

        self.btnHome.clicked.connect(self.onBtnHome)
        self.btnZero.clicked.connect(self.onBtnZero)
        self.btnStop.clicked.connect(self.onBtnStop)
        self.pushButto_move.clicked.connect(self.onBtnMove)
    
    ########################
    ### GCode 测试函数 Start
    def onBtnHome(self):
        msg = f'$h'
        self.send_data_(msg)

        # ### 处理接收到的数据
        # msg = self.client_socket.recv(1024).decode("utf-8")
        # print(f"recv data is {msg}")
        # recvdata_json = json.loads(msg)
        # self.textEdit.setText(str(recvdata_json))

    def onBtnZero(self):
        msg = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000'
        self.send_data_(msg)
        
        # ### 处理接收到的数据
        # msg = self.client_socket.recv(1024).decode("utf-8")
        # recvdata_json = json.loads(msg)
        # self.textEdit.setText(str(recvdata_json))
    
    def onBtnStop(self):
        msg = f'!'
        self.send_data_(msg)

        time.sleep(0.5)
        msg = f'%'
        self.send_data_(msg)

        # ### 处理接收到的数据
        # msg = self.client_socket.recv(1024).decode("utf-8")
        # print(f"recv data is {msg}")
        # recvdata_json = json.loads(msg)
        # self.textEdit.setText(str(recvdata_json))

    def onBtnMove(self):
        P = self.updateValue2Lubancat()
        Gcode = self.move(P=P,coordinate=0)

        self.send_data_(Gcode)

        # ### 处理接收到的数据
        # msg = self.client_socket.recv(1024).decode("utf-8")
        # print(f"recv data is {msg}")
        # recvdata_json = json.loads(msg)
        # self.textEdit.setText(str(recvdata_json))
    ### GCode 测试函数  End
    ########################

    def onStateChanged(self):
        if self.btnConnect.text() == '未连接':  
            # self.btnConnect.setStyleSheet("background-color:gray")
            # self.send_data_init()
            self.invalid_IP_Port_Connect()

            if self.flag == True:
                self.btnConnect.setText('已连接')
        else:  
            self.disConnected()
            self.btnConnect.setText('已断开')            

    def disConnected(self):
        self.client_socket.close()

    def _combox(self):
        self.comboBox.addItem("库函数")
        self.comboBox.addItem("复位")  
        self.comboBox.addItem("回零复位")  
        self.comboBox.addItem("吸盘开")
        self.comboBox.addItem("吸盘关")  
        self.comboBox.addItem("吸盘吹气")  
        self.comboBox.addItem("滑轨移动")
        self.comboBox.setView(QListView())
        self.comboBox.setStyleSheet("QComboBox QAbstractItemView::item{height:22px;}")

    def onButtonClick(self):  
        # 获取下拉框的选中项  
    
        index = self.comboBox.currentIndex()  
        if index >= 0:  
            selectedItem = self.comboBox.currentText()
            #### python3.10以上才有match
            # patterns = {
            #     r"复位":"arm.home()",
            #     r"回零复位":"arm.go_to_zero()",
            # }
            # for pattern, action in patterns.items():
            #     if match(selectedItem,pattern):
            #         self.textEdit.append(action)
            #         break
            #     else:
            #         self.textEdit.append("未知指令")

            # 根据选中的项发送指令到文本编辑器中并插入一行  
            if selectedItem == "复位":  
                self.textEdit.append("arm.home()")
            elif selectedItem == "库函数":
                self.textEdit.append("from wlkata_mirobot import WlkataMirobot")
                self.textEdit.append("arm = WlkataMirobot()")  
            elif selectedItem == "回零复位":  
                self.textEdit.append("arm.go_to_zero()")  
            elif selectedItem == "指令3":  
                self.textEdit.append("这是指令3")  
            else:  
                self.textEdit.append("未知指令")   
                   
    def execute_code(self):
        contents = self.textEdit.toPlainText()
        try:
            with open('temp.py', 'w') as file:
                file.write(contents)

                exec(open('temp.py').read())
        except Exception as e:
            print(f"fasheng cuowue ", str(e))

    def stop_code(self):
        contents = self.textEdit.toPlainText()
        try:
            with open('temp.py', 'w') as file:
                file.write(contents)

                exit(open('temp.py').read())
        except Exception as e:
            print(f"fasheng cuowue ", str(e))

    def clear_code(self):
        self.textEdit.clear()

    def onBtnStatus(self):
        self.global_state
        print(f"How easy to get status")

    def onSliderReleased(self, Label):
        try:
            value = self.sender().value()
            Label.setText(str(value))
            QApplication.processEvents()
            print(f"updateValue2Lubancat: {value}") 
            self.updateValue2Lubancat()
        except ValueError:
            print(f"slider Value is Error: {value}")     

    def updateValue2Lubancat(self):
        if self.btnConnect.text() == '已连接':
            server_IP = str(self.lineEdit_9.text())
            server_port = int(self.lineEdit_10.text())

            state = self.label_status.text()

            angles_0 = self.lineEdit_1.text()
            angles_1 = self.lineEdit_2.text()
            angles_2 = self.lineEdit_3.text()
            angles_3 = self.lineEdit_4.text()
            angles_4 = self.lineEdit_5.text()
            angles_5 = self.lineEdit_6.text()
            self.angles_6 = self.lineEdit_7.text()
            P = [angles_0, angles_1, angles_2, angles_3, angles_4, angles_5]

            cartesians_x = self.lineEdit_1world.text()
            cartesians_y = self.lineEdit_2world.text()
            cartesians_z = self.lineEdit_3world.text()
            cartesians_a = self.lineEdit_4world.text()
            cartesians_b = self.lineEdit_5world.text()
            cartesians_c = self.lineEdit_6world.text()
            return P

        else:
            print(f"Please connect TCP/IP to server!!") 

    def receive_data_updateUI(self, msg):
        if msg == "ok":
            QApplication.processEvents()
            self.client_socket.close()
        else:
            result_value = msg["deviceData"]["angle"]
            angle_x = result_value[0]
            angle_y = result_value[1]
            angle_z = result_value[2]
            angle_a = result_value[3]
            angle_b = result_value[4]
            angle_c = result_value[5]
            angle_d = result_value[6]
                        
            coordinate_value = msg["deviceData"]["coordinate"]
            cartesians_x = coordinate_value[0]
            cartesians_y = coordinate_value[1]
            cartesians_z = coordinate_value[2]
            cartesians_a = coordinate_value[3]
            cartesians_b = coordinate_value[4]
            cartesians_c = coordinate_value[5]

            pump_pwm = msg["deviceData"]["pwm"]
            motion_mode = msg["deviceData"]["mode"]
            self.global_state = msg["deviceData"]["state"]

            ### slider setValue
            self.horizontalSlider_1.setValue(int(float(angle_x)))
            self.horizontalSlider_2.setValue(int(float(angle_y)))
            self.horizontalSlider_3.setValue(int(float(angle_z)))
            self.horizontalSlider_4.setValue(int(float(angle_a)))
            self.horizontalSlider_5.setValue(int(float(angle_b)))
            self.horizontalSlider_6.setValue(int(float(angle_c)))
            self.horizontalSlider_7.setValue(int(float(angle_d)))

            self.lineEdit_1.setText(angle_x)
            self.lineEdit_2.setText(angle_y)
            self.lineEdit_3.setText(angle_z)
            self.lineEdit_4.setText(angle_a)
            self.lineEdit_5.setText(angle_b)
            self.lineEdit_6.setText(angle_c)
            self.lineEdit_7.setText(angle_d)
            self.label_status.setText(self.global_state)

            self.horizontalSlider_1world.setValue(int(float(cartesians_x)))
            self.horizontalSlider_2world.setValue(int(float(cartesians_y)))
            self.horizontalSlider_3world.setValue(int(float(cartesians_z)))
            self.horizontalSlider_4world.setValue(int(float(cartesians_a)))
            self.horizontalSlider_5world.setValue(int(float(cartesians_b)))
            self.horizontalSlider_6world.setValue(int(float(cartesians_c)))

            self.lineEdit_1world.setText(cartesians_x)
            self.lineEdit_2world.setText(cartesians_y)
            self.lineEdit_3world.setText(cartesians_z)
            self.lineEdit_4world.setText(cartesians_a)
            self.lineEdit_5world.setText(cartesians_b)
            self.lineEdit_6world.setText(cartesians_c)
            QApplication.processEvents()
            self.client_socket.close()


    ### 虚拟控制器测试代码--单元
    # 定义通讯协议格式  
    def build_packet(self,data_size, packet_type, message_body):  
        # 构建报头  
        header = json.dumps({  
            "data_size": data_size,  
            "type": packet_type  
        }).encode('utf-8')
        
        # 计算报头长度（4位bit，即半个字节，需要转换为字节长度）  
        header_length = len(header)
        print(header_length)
        
        # 构建帧头
        # frame_header = struct.pack('>H', header_length)
        frame_header = header_length.to_bytes(4, 'little')
        print(frame_header)
        
        # 完整的数据包  
        packet = frame_header + header + message_body.encode('utf-8')
        return packet 

    # 定义消息体  
    def create_message_body(self,robot_type, state, com1, com2, rs485, com1Float, com2Float,io):  
        message_body = {  
            "robot_type": robot_type,  
            "state": state,  
            "com1": com1,  
            "com2": com2,  
            "RS485": rs485, 
            "com1Float": com1Float,
            "com2Float": com2Float,
            "IO": io  
        }  
        return json.dumps(message_body)  

    def invalid_IP_Port_Connect(self):
        self.flag = False
        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  
        server_IP = str(self.lineEdit_9.text())
        if server_IP == '':
            print(f"Please check ip is NULL")
            return
         
        server_port = int(self.lineEdit_10.text())
        # if server_port is None:
        if str(self.lineEdit_10.text()) == '':  
            print(f"Please check port is NULL")
            return

        if len(server_IP) != 0 and not server_port is None:
            try:
                ip_pattern = r'^((25[0-5]|2[0-4]\d|1\d{2}|[1-9]\d|\d)\.){3}(25[0-5]|2[0-4]\d|1\d{2}|[1-9]\d|\d)$'  
                if not re.match(ip_pattern, server_IP):  
                    return False, 'Invalid IP address'  
                
                self.client_socket.connect((server_IP, server_port))
                self.flag = True
                print(f"Connected to {server_IP}:{server_port}")     
            
            except ValueError:
                print(f"Please input the right ip/port")
                return


    def send_data_init(self):
        try:
            ### 校验IP/PORT
            self.invalid_IP_Port_Connect()
            ### G代码，询问机器人状态
            data = "?"
            self.flag = True
            self.send_data_(data)
            
            ### 处理接收到的数据
            msg = self.client_socket.recv(1024).decode("utf-8")
            recvdata_json = json.loads(msg)
            self.textEdit.setText(str(recvdata_json))

        except Exception as e:
            print(e)

    def validJog_Cartians_Speed(self, text):
        try:
            value = float(text)
            if value < -100 or value > 160:
                self.lineEdit.clear()
                print ("Value out of range")
        except ValueError:
            print()

    def send_data_(self,data):
        robot_type = self.robot_type
        state = self.global_state
        com1 = data
        com2 = data
        rs484 = ""
        com1Float = self.angles_6 
        com2Float = self.angles_6 
        io = self.IO
        
        ### 消息体
        message_body = self.create_message_body(robot_type, state, com1, com2, rs484, com1Float, com2Float, io)
        ### 消息体长度
        data_size = len(message_body)
        ### 控制器类型：虚拟控制器
        packet_type = "virtual" 
        ### 构建数据包 
        packet = self.build_packet(data_size, packet_type, message_body)
        self.client_socket.sendall(packet)


    ### G代码指令转换

    def home(self, has_slider=False):
        '''机械臂Homing'''
        if has_slider:
            return self.home_7axis()
        else:
            print("home")
            return self.home_6axis()

    def home_1axis(self, axis_id):
        '''单轴Homing'''
        if not isinstance(axis_id, int) or not (axis_id >= 1 and axis_id <= 7):
            return False
        msg = f'$h{axis_id}'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis(self):
        '''六轴Homing'''
        msg = f'$h'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis_in_turn(self):
        '''六轴Homing, 各关节依次Homing'''
        msg = f'$hh'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
    
    def stop(self):
        '''6轴暂停'''
        # msg = f'!'
        # self.send_msg(msg, wait_ok=False, wait_idle=False)
        # time.sleep(0.5)
        msg = f'%'
        return self.send_msg(msg, wait_ok=False, wait_idle=False)

    def home_7axis(self):
        '''七轴Homing(本体 + 滑台)'''
        msg = f'$h0'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
        
    def unlock_all_axis(self):
        '''解锁各轴锁定状态'''
        msg = 'M50'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)
        
    def go_to_zero(self):
        '''回零-运动到名义上的各轴零点'''
        msg = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def set_speed(self, speed):
        '''设置转速'''
        # 转换为整数
        speed = int(speed)
        # 检查数值范围是否合法
        if speed <= 0 or speed > 6000:
            robot_logger.error(f"Illegal movement speed {speed}")
            return False
        # 发送指令
        msg = f'F{speed}'
        return self.send_msg(msg, wait_ok=None, wait_idle=None)

    def set_hard_limit(self, enable):
        '''
        开启硬件限位
        '''
        msg = f'$21={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def set_soft_limit(self, enable):
        '''开启软限位
        注: 请谨慎使用
        '''
        msg = f'$20={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)



    def format_float_value(self, value):
        if value is None:
            return value
        if isinstance(value, float):
            # 精确到小数点后两位数
            return round(value , 2)
        else:
            return value

    def generate_args_string(self, instruction, pairings):
        '''生成参数字符'''
        args = [f'{arg_key}{self.format_float_value(value)}' for arg_key, value in pairings.items() if value is not None]

        return ' '.join([instruction] + args)
        
    def move(self, P, speed = None ,coordinate= 0, is_relative=0,mode =0, wait_ok=None):

        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        instruction = 'M20 G90 G1'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        m1 = "M20"             # 世界坐标系
        pairings = {'X': P[0], 'Y': P[1], 'Z': P[2], 'A': P[3], 'B': P[4], 'C': P[5], 'F': speed}
        if coordinate ==0:
            m1 = "M21"      # 轴角坐标系
        m2 = "G90"
        if is_relative:
            m2 = 'G91'       
        m3 ="G00"
        if mode == 1:
            m3 = "G01"

        instruction = f"{m1} {m2} {m3} "
        
        msg = self.generate_args_string(instruction, pairings)
        print(msg)
        return msg

    ### G代码指令转换

if __name__== "__main__":
    app = QApplication(sys.argv)
    MainWindow = MainWindow_main_zubo()
    MainWindow.setWindowIcon(QIcon("./icon.jpg"))
    MainWindow.setWindowTitle("KT-Robot Demo")
    MainWindow.show()
    sys.exit(app.exec_())


###### Client in Machine order to UI Operator to control KT-Robot (JSON) 
###### Author: ZHENG
###### Date: 2023-12-7
###### END
